#include <iostream>
#include <iomanip>
#include <boost/asio.hpp>
#include <boost/bind/bind.hpp>
#include "server.h"

using boost::asio::ip::tcp;

Server::Server(boost::asio::io_context& io_context, short port)
    : acceptor_(io_context, tcp::endpoint(tcp::v4(), port)),
      socket_(io_context) {
    start_accept();
}

void Server::start_accept() {
    acceptor_.async_accept(socket_,
        boost::bind(&Server::handle_accept, this,
        boost::asio::placeholders::error));
}

void Server::handle_accept(const boost::system::error_code& error) {
    if (!error) {
        std::cout << "Connection accepted!" << std::endl;
        start_receive();
    } else {
        std::cerr << "Error in accepting connection: " << error.message() << std::endl;
    }
}

void Server::start_receive() {
    socket_.async_read_some(boost::asio::buffer(data_, max_length),
        boost::bind(&Server::handle_receive, this,
        boost::asio::placeholders::error,
        boost::asio::placeholders::bytes_transferred));
}

void Server::handle_receive(const boost::system::error_code& error, std::size_t bytes_transferred) {
    if (!error) {
        // std::cout << "Received data: ";
        // for (std::size_t i = 0; i < bytes_transferred; ++i) {
        //     std::cout << std::hex << std::setw(2) << std::setfill('0') << (static_cast<int>(data_[i]) & 0xff) << " ";
        // }
        // std::cout << std::dec << std::endl; // 切换回十进制

        // Process the received data
        process_data(data_, bytes_transferred);
        
        // Continue receiving data
        start_receive();
    } else {
        std::cerr << "Error in receiving data: " << error.message() << std::endl;
        socket_.close();
    }
}

void Server::process_data(const uint8_t* data, std::size_t length) {
    Re_data Redata[32];

    // Assuming each Re_data structure is packed in 13 bytes, and first 7 bytes are metadata
    size_t data_index = 7;
    for (int i = 0; i < 32; ++i) {
        if (data_index + 13 <= length) {
            Redata[i].TargetID = data[data_index];
            // 预留位 横向速度 Cross-track velocity m/s
            Redata[i].rav3 = static_cast<float>(static_cast<int16_t>((data[data_index + 2] << 8) | data[data_index + 1])) / 100.0f;
            // 目标距离 径向距离 Range m
            Redata[i].rav1 = static_cast<float>(static_cast<int16_t>((data[data_index + 4] << 8) | data[data_index + 3])) / 10.0f;
            // 角度 水平方位角 Azimuth °
            Redata[i].rav2 = static_cast<float>(static_cast<int16_t>((data[data_index + 6] << 8) | data[data_index + 5])) / 100.0f;
            // Y轴距离 纵向距离 Along-track distance m
            Redata[i].xyz2 = static_cast<float>(static_cast<int16_t>((data[data_index + 8] << 8) | data[data_index + 7])) / 10.0f;
            // X轴距离 横向距离 Cross-track distance m
            Redata[i].xyz1 = static_cast<float>(static_cast<int16_t>((data[data_index + 10] << 8) | data[data_index + 9])) / 10.0f;
            // 速度 径向速度 Range rate m/s
            Redata[i].rav4 = static_cast<float>(static_cast<int16_t>((data[data_index + 12] << 8) | data[data_index + 11])) / 100.0f;

            data_index += 13;
        } else {
            break;
        }
    }

    // Log received data
    for (int i = 0; i < 32; ++i) {
        if (Redata[i].TargetID != 0) {
            std::cout << "TargetID: " << static_cast<int>(Redata[i].TargetID)
                      << " Azimuth: " << Redata[i].rav2
                      << " Cross-track distance: " << Redata[i].xyz1
                      << " Cross-track velocity: " << Redata[i].rav3
                      << " Along-track distance: " << Redata[i].xyz2
                      << " Range: " << Redata[i].rav1
                      << " Range rate: " << Redata[i].rav4 << std::endl;
        }
    }
}
